/*
 * aniso_rof_denoising.c
 *
 *  Created on: Feb 6, 2014
 *      Author: rahul
 */

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include"allocation.h"
#include "img_read.h"
#include "sorting.h"
#include "estdlib.h"
#include "ip.h"
#include "matrix_op.h"
#include "nakagami_param_estimation.h"
int Window=5;
float eta=0.1;
float lambda=0.35;
float** error_calc(float **f, float **u, float s, float omega, int im_height, int im_width)
{
	int r_indx, c_indx;
	float **hf;
	hf=alloc2Dfloat(im_height, im_width);

	for(r_indx=1;r_indx<im_height-1;r_indx++)
	{
		for(c_indx=1;c_indx<im_width-1;c_indx++)
		{
			hf[r_indx][c_indx]=(omega-exp(2*s*(f[r_indx][c_indx]-u[r_indx][c_indx])));
		}
	}
img_write_pgm_f(hf, "hf_im1.pgm");
	return hf;
}

float **x_estimation()

float ** regularizer(float** u_im, float **cq, int im_height, int im_width)
{
	int r_indx, c_indx;
	float **hp, **Ilap;
	hp=alloc2Dfloat(im_height, im_width);
	Ilap=cal_lap(u_im, im_height, im_width);
	for(r_indx=1;r_indx<im_height-1;r_indx++)
	{
		for(c_indx=1;c_indx<im_width-1;c_indx++)
		{
			hp[r_indx][c_indx]=(cq[r_indx+1][c_indx])*(u_im[r_indx+1][c_indx]-u_im[r_indx][c_indx])+(cq[r_indx][c_indx])*(u_im[r_indx-1][c_indx]-u_im[r_indx][c_indx])+(cq[r_indx][c_indx+1])*(u_im[r_indx][c_indx+1]-u_im[r_indx][c_indx])+(cq[r_indx][c_indx])*(u_im[r_indx][c_indx]-u_im[r_indx][c_indx-1]);
		}
	}
img_write_pgm_f(hp, "hp_im1.pgm");
	return hp;
}
/*************calculate q0******************/
float calculate_cmad(float **u, int im_height, int im_width)
{
	int r_indx, c_indx, w_indx, w_indy, w_size, counter;
	float cu, **Ix, **Iy, **log_u, **mag, *vec, **temp_med, **tempIx, **tempIy, **mag_new;

	w_size=(int)floor((float)Window/2);
	temp_med=alloc2Dfloat(im_height, im_width);
	tempIx=alloc2Dfloat(im_height, im_width);
	tempIy=alloc2Dfloat(im_height, im_width);
	mag_new=alloc2Dfloat(im_height, im_width);
	log_u=alloc2Dfloat(im_height, im_width);
	mag=alloc2Dfloat(im_height, im_width);
	for(r_indx=0;r_indx<im_height;r_indx++)
	{
		for(c_indx=0;c_indx<im_width;c_indx++)
		{
			log_u[r_indx][c_indx]=log(u[r_indx][c_indx]+1);
		}
	}
	Ix=calgradx(log_u, im_height, im_width);
	Iy=calgrady(log_u, im_height, im_width);
	for(r_indx=0;r_indx<im_height;r_indx++)
	{
		for(c_indx=0;c_indx<im_width;c_indx++)
		{
			mag[r_indx][c_indx]=sqrt(Ix[r_indx][c_indx]*Ix[r_indx][c_indx]+Iy[r_indx][c_indx]*Iy[r_indx][c_indx]);
		}
	}
	for(r_indx=w_size;r_indx<im_height-w_size;r_indx++)
	{
		for(c_indx=w_size;c_indx<im_width-w_size;c_indx++)
		{
			vec=alloc1Dfloat(Window*Window);
			for(w_indx=-w_size, counter=0;w_indx<w_size+1;w_indx++)
			{
				for(w_indy=-w_size;w_indy<w_size+1;w_indy++,counter++)
				{
					vec[counter]=mag[r_indx+w_indx][c_indx+w_indy];
				}
			}
			temp_med[r_indx][c_indx]=median_func(vec, Window*Window);
			tempIx[r_indx][c_indx]=Ix[r_indx][c_indx]-temp_med[r_indx][c_indx];
			tempIy[r_indx][c_indx]=Iy[r_indx][c_indx]-temp_med[r_indx][c_indx];
			mag_new[r_indx][c_indx]=sqrt(Ix[r_indx][c_indx]*Ix[r_indx][c_indx]+Iy[r_indx][c_indx]*Iy[r_indx][c_indx]);

		}
	}
	cu=1.04836*median_func2D(mag_new,im_height,im_width);
	return cu;




}

/*************function to calculate instantaneous coefficient q******************/
float **instant_coefficient(float **u, int im_height, int im_width)
{
	int r_indx, c_indx;
	float **Ix, **Iy, **I_lap, **mag, **q, q0, **cq, **tempq;
	Ix=calgradx(u, im_height, im_width);
	Iy=calgrady(u, im_height, im_width);
	I_lap=cal_lap(u, im_height, im_width);
	q=alloc2Dfloat(im_height, im_width);
	cq=alloc2Dfloat(im_height, im_width);
	mag=alloc2Dfloat(im_height,im_width);
	tempq=alloc2Dfloat(im_height,im_width);
	q0=calculate_cmad(u, im_height, im_width);
	for(r_indx=1;r_indx<im_height-1;r_indx++)
	{
		for(c_indx=1;c_indx<im_width-1;c_indx++)
		{
			mag[r_indx][c_indx]=(Ix[r_indx][c_indx]*Ix[r_indx][c_indx]+Iy[r_indx][c_indx]*Iy[r_indx][c_indx]);
			mag[r_indx][c_indx]=(float)mag[r_indx][c_indx]/(u[r_indx][c_indx]*u[r_indx][c_indx]+0.001);
			I_lap[r_indx][c_indx]=(float)I_lap[r_indx][c_indx]/(u[r_indx][c_indx]+0.001);
			q[r_indx][c_indx]=(float)(0.5*mag[r_indx][c_indx]-0.0625*(I_lap[r_indx][c_indx]*I_lap[r_indx][c_indx]))/((1+0.25*I_lap[r_indx][c_indx])*(1+0.25*I_lap[r_indx][c_indx])+0.001);
			tempq[r_indx][c_indx]=fabs((float)(q[r_indx][c_indx]-q0)/(q0*(1+q0)));
			cq[r_indx][c_indx]=(float)exp(-(double)(tempq[r_indx][c_indx]));

		}
	}

	img_write_pgm_f(q, "q_im1.pgm");
	img_write_pgm_f(tempq, "q_im2.pgm");
	return cq;
}

float **gradient_desent_update(float **f, float **u, float **c_q, float m_est, float s_est, float omega_est, int im_height, int im_width)
{
	int r_indx, c_indx;
	float **hp, **hf;
	hf=error_calc(f, u,s_est, omega_est, im_height, im_width);
	hp=regularizer(u, c_q, im_height, im_width);
	for(r_indx=1;r_indx<im_height-1;r_indx++)
	{
		for(c_indx=1;c_indx<im_width-1;c_indx++)
		{
			u[r_indx][c_indx]=u[r_indx][c_indx]+(eta*(hf[r_indx][c_indx]+lambda*hp[r_indx][c_indx]));
		}
	}

return u;
}
